
#include "PreHeader.h"
#include "cCatmullRomSpline.h"

#define M11  0.0f
#define M12  1.0f
#define M13  0.0f
#define M14  0.0f
#define M21 -0.5f
#define M22  0.0f
#define M23  0.5f
#define M24  0.0f
#define M31  1.0f
#define M32 -2.5f
#define M33  2.0f
#define M34 -0.5f
#define M41 -0.5f
#define M42  1.5f
#define M43 -1.5f
#define M44  0.5f

b2Vec2 cCatmullRomSpline::Interpolate(tFloat time, const tPointArray4& pointArray4)
{
	tFloat c1, c2, c3, c4;
	b2Vec2 point;

	const b2Vec2& p1 = pointArray4[0];
	const b2Vec2& p2 = pointArray4[1];
	const b2Vec2& p3 = pointArray4[2];
	const b2Vec2& p4 = pointArray4[3];

	c1 = M12*p2.x;   
	c2 = M21*p1.x + M23*p3.x;   
	c3 = M31*p1.x + M32*p2.x + M33*p3.x + M34*p4.x;   
	c4 = M41*p1.x + M42*p2.x + M43*p3.x + M44*p4.x;
	point.x = (((c4*time + c3)*time +c2)*time + c1);

	c1 = M12*p2.y;
	c2 = M21*p1.y + M23*p3.y;   
	c3 = M31*p1.y + M32*p2.y + M33*p3.y + M34*p4.y;   
	c4 = M41*p1.y + M42*p2.y + M43*p3.y + M44*p4.y;   
	point.y = (((c4*time + c3)*time +c2)*time + c1);

	return point;
}